#include "kalmanfilter.h"

/*卡尔曼滤波初始化*/
void kalmanfilter_init(kalmanfilter_t *kf, float q, float e_mea, float initial_value)
{
    /*过程噪声方差*/
    kf->q = q;
    /*测量噪声方差*/
    kf->e_mea = e_mea;
    /*初始估计值*/
    kf->x = initial_value;
    /*初始估计误差*/
    kf->e_est = 5.0;
    /*初始卡尔曼增益*/
    kf->k = 0.0;
}

/*根据传入的测量值，预测下一次的估计值*/
void kalmanfilter_predict(kalmanfilter_t *kf, volatile uint16_t *measurement)
{
    /*计算卡尔曼滤波增益:卡尔曼增益=上一次的估计误差/(上一次的估计误差+测量噪声误差)*/
    kf->k = kf->e_est / (kf->e_est + kf->e_mea);
    /*计算本次估计值:本次估计值=上一次的估计值+卡尔曼增益*(本次测量值-上一次的估计值)*/
    /*每次传入新的测量值，得到的kf->x为预测的下一次的估计值*/
    kf->x = kf->x + kf->k * (*measurement - kf->x);
    /*更新本次估计误差:本次估计误差=(1-卡尔曼增益)*上一次的估计误差 + 过程噪声误差*/
    kf->e_est = (1 - kf->k) * kf->e_est + kf->q;
}
